/**
 * @file dvc_manifold.cpp
 * @author yssickjgd (yssickjgd@mail.ustc.edu.cn)
 * @brief 妙算视觉
 * @version 0.1
 * @date 2022-08-13
 *
 * @copyright USTC-RoboWalker (c) 2022
 *
 */

/* Includes ------------------------------------------------------------------*/

#include "dvc_manifold.h"

/* Private macros ------------------------------------------------------------*/

/* Private types -------------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

/* Private function declarations ---------------------------------------------*/

/**
 * @brief 妙算初始化
 * 
 * @param __huart 指定的UART
 * @param __Frame_Header 数据包头标
 */
void Class_Manifold::Init(UART_HandleTypeDef *__huart, uint16_t __Frame_Header)
{
    huart = __huart;
    Frame_Header = __Frame_Header;
    if (huart->Instance == USART1)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART1_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == USART2)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART2_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == USART3)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART3_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == UART4)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART4_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == UART5)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART5_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == USART6)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART6_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == UART7)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART7_Manage_Object.Rx_Buffer;
    }
    else if (huart->Instance == UART8)
    {
        UART_Rx_Data = (Struct_Manifold_UART_Data *)UART8_Manage_Object.Rx_Buffer;
    }
}

/**
 * @brief 重置接收数据
 *
 */
void Class_Manifold::Reset_UART_Rx_Data()
{
    UART_Rx_Data->Delta_Yaw = 0;
    UART_Rx_Data->Delta_Pitch = 0;
    UART_Rx_Data->Predicted_Delta_Yaw = 0;
    UART_Rx_Data->Predicted_Delta_Pitch = 0;
    UART_Rx_Data->Distance = 0;
    UART_Rx_Data->Predicted_Velocity_Yaw = 0;
    UART_Rx_Data->Predicted_Velocity_Pitch = 0;
    UART_Rx_Data->Data_Flag = 0;
}

/**
 * @brief 数据处理过程
 *
 */
void Class_Manifold::Data_Process()
{

    //数据处理过程
    Data.Data_Flag = UART_Rx_Data->Data_Flag;
    Data.Delta_Yaw = UART_Rx_Data->Delta_Yaw / 18000.0f * PI;
    Data.Delta_Pitch = UART_Rx_Data->Delta_Pitch / 18000.0f * PI;
    Data.Predicted_Delta_Yaw = UART_Rx_Data->Predicted_Delta_Yaw / 18000.0f * PI;
    Data.Predicted_Delta_Pitch = UART_Rx_Data->Predicted_Delta_Pitch / 18000.0f * PI;
    Data.Predicted_Velocity_Yaw = UART_Rx_Data->Predicted_Velocity_Yaw / 18000.0f * PI;
    Data.Predicted_Velocity_Pitch = UART_Rx_Data->Predicted_Velocity_Pitch / 18000.0f * PI;
    Data.Distance = UART_Rx_Data->Distance / 1000.0f;
}

/**
 * @brief 获取标志位
 * 
 * @return float 距离
 */
uint16_t Class_Manifold::Get_Data_Flag()
{
    return (Data.Data_Flag);
}

/**
 * @brief 获取航向偏差
 * 
 * @return float 航向偏差
 */
float Class_Manifold::Get_Delta_Yaw()
{
    return (Data.Delta_Yaw);
}

/**
 * @brief 获取俯仰偏差
 * 
 * @return float 俯仰偏差
 */
float Class_Manifold::Get_Delta_Pitch()
{
    return (Data.Delta_Pitch);
}

/**
 * @brief 获取预测航向偏差
 * 
 * @return float 预测航向偏差
 */
float Class_Manifold::Get_Predicted_Delta_Yaw()
{
    return (Data.Predicted_Delta_Yaw);
}

/**
 * @brief 获取预测俯仰偏差
 * 
 * @return float 预测俯仰偏差
 */
float Class_Manifold::Get_Predicted_Delta_Pitch()
{
    return (Data.Predicted_Delta_Pitch);
}

/**
 * @brief 获取预测航向速度
 * 
 * @return float 预测航向速度
 */
float Class_Manifold::Get_Predicted_Velocity_Yaw()
{
    return (Data.Predicted_Velocity_Yaw);
}

/**
 * @brief 获取预测俯仰速度
 * 
 * @return float 预测俯仰速度
 */
float Class_Manifold::Get_Predicted_Velocity_Pitch()
{
    return (Data.Predicted_Velocity_Pitch);
}

/**
 * @brief 获取距离
 * 
 * @return float 距离
 */
float Class_Manifold::Get_Distance()
{
    return (Data.Distance);
}

/**
 * @brief TIM定时器中断定期检测妙算是否存活
 *
 */
void Class_Manifold::TIM_Alive_PeriodElapsedCallback()
{
    //判断该时间段内是否接收过遥控器数据
    if (Flag == Pre_Flag)
    {
        //遥控器断开连接
        Manifold_Status = Manifold_Status_DISABLE;
        Reset_UART_Rx_Data();
        Data_Process();
    }
    else
    {
        //遥控器保持连接
        Manifold_Status = Manifold_Status_ENABLE;
    }
    Pre_Flag = Flag;
}

/**
 * @brief UART通信接收回调函数
 *
 * @param Rx_Data 接收的数据
 */
void Class_Manifold::UART_RxCpltCallback(uint8_t *Rx_Data)
{
    //滑动窗口, 判断遥控器是否在线
    Flag += 1;

    Data_Process();
}

/* Function prototypes -------------------------------------------------------*/

/************************ COPYRIGHT(C) USTC-ROBOWALKER **************************/
